/* Gluten Library -- Matrix
 *
 * A templated and operator overloaded MxN row-major matrix type. Supports 
 * most of the usual matrix operations.
 *
 * Currently, there is a optimization concern with matrix multiplication. If
 * you need to multiply matrices that differ by storage-type, try to make sure
 * that the first matrix has the same storage type as the matrix the result
 * will be stored in or else it will have to perform an extra cast on the 
 * result.
 *
 * For example:
 *    Matrix3i a(...)
 *    Matrix3d b(...)
 *    Matrix3d c(a * b)
 *
 * This will result in an extra cast because the final result of a * b is of
 * type Matrix3i, which now has to be cast to Matrix3d before storing. This
 * won't always be an option, matrix multiplication not being commutatitve, but
 * where possible...
 *
 * $AUTHOR$    res
 * $UPDATE$    r27
 */

/* LICENSES ********************************************************************

Copyright (c) 2013- Reuben E. Smith

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

*******************************************************************************/

// TODO Left and right matrix division.
// TODO 4x4 det and invert? NxN det and invert?
// TODO SIMD? This will likely require library rework...


#ifndef GLUTEN_MATRIX_HPP
#define GLUTEN_MATRIX_HPP

#ifdef DEBUG
   #include <cassert>
#endif

#include <cstring>
#include "Utility.hpp"
#include "Vector.hpp"


namespace Gluten
{
   //! \class Matrix
   //! \brief An MxN matrix using variable storage type.
   //! \param[in]    M     row count
   //! \param[in]    N     column count
   //! \param[in]    T     cell storage type
   template < unsigned int M, unsigned int N, typename T >
   class Matrix
   {
   public:
      typedef T value_t;
      static const unsigned int rows = M;
      static const unsigned int columns = N;


      //! \brief Default constructor. Zeros out matrix.
      Matrix()
      {
         for (unsigned int i = 0; i < M; ++i) {
            std::memset(_m[i], 0, N * sizeof(T));
         }
      }


      //! \brief Copy constructor. Copies values from another matrix.
      //! \param[in]    other          the matrix to copy
      Matrix(const Matrix& other)
      {
         for (unsigned int i = 0; i < M; ++i) {
            std::memcpy(_m[i], other._m[i], N * sizeof(T));
         }
      }


      //! \brief Array constructor. Copies values from an array.
      //! \param[in]    values         the array to copy
      template < typename U >
      explicit Matrix(const U values[M][N])
      {
         for (unsigned int i = 0; i < M; ++i) {
            std::memcpy(_m[i], values[i], N * sizeof(T));
         }
      }


      //! \brief Get a zero matrix.
      //! \return       a matrix with all cells 0
      static Matrix Zero()
      {
         // res: Why did I think this was a useful thing to have...?
         return Matrix();
      }


      //! \brief Get a ones matrix.
      //! \return       a matrix with all cells 1
      static Matrix One()
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r(i, j) = 1;
            }
         }

         return r;
      }


      //! \brief Get an identity matrix based on this matrix's row size.
      //! \return       an MxM identity matrix
      static Matrix RowIdentity()
      {
         Matrix<M, M, T> r;

         for (unsigned int i = 0; i < M; ++i) {
            r(i, i) = 1;
         }

         return r;
      }


      //! \brief Get an identity matrix based on this matrix's column size.
      //! \return       an NxN identity matrix
      static Matrix ColumnIdentity()
      {
         Matrix<N, N, T> r;

         for (unsigned int i = 0; i < N; ++i) {
            r(i, i) = 1;
         }

         return r;
      }


      //! \brief Test if matrix is square or not.
      //! \return       true iff M = N
      static bool isSquare()
      {
         return M == N;
      }


      //! \brief Get the determinant for a 2x2 matrix.
      //! \return       the 2x2 matrix's determinant
      static T det(const Matrix<2, 2, T>& m)
      {
         return m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
      }


      //! \brief Get the determinant for a 3x3 matrix.
      //! \return       the 3x3 matrix's determinant
      static T det(const Matrix<3, 3, T>& m)
      {
         return m(0, 0) * m(1, 1) * m(2, 2) +
                m(0, 1) * m(1, 2) * m(2, 0) +
                m(0, 2) * m(1, 0) * m(2, 1) -
                m(0, 2) * m(1, 1) * m(2, 0) -
                m(0, 1) * m(1, 0) * m(2, 2) -
                m(0, 0) * m(1, 2) * m(2, 1);
      }


      //! \brief Get the inverse for a 2x2 matrix.
      //! \return       a new matrix, the inverse of the original 2x2 matrix if
      //!               it exists; zero matrix otherwise
      static Matrix<2, 2, T> inverse(const Matrix<2, 2, T>& m)
      {
         Matrix<2, 2, T> r;
         double d = Matrix<2, 2, T>::det(m);

         if (!fpRelativeEqual(d, 0.0)) {
            d = 1.0 / d;
            r(0, 0) = m(1, 1);
            r(0, 1) = -m(0, 1);
            r(1, 0) = -m(1, 0);
            r(1, 1) = m(0, 0);
            r *= d;
         }

         return r;
      }


      //! \brief Get the inverse for a 3x3 matrix.
      //! \return       a new matrix, the inverse of the original 3x3 matrix if
      //!               it exists; zero matrix otherwise
      static Matrix<3, 3, T> inverse(const Matrix<3, 3, T>& m)
      {
         Matrix<3, 3, T> r;
         double d = Matrix<3, 3, T>::det(m);

         if (!fpRelativeEqual(d, 0.0)) {
            d = 1.0 / d;
            r(0, 0) = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1);
            r(0, 1) = m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2);
            r(0, 2) = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
            r(1, 0) = m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2);
            r(1, 1) = m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0);
            r(1, 2) = m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2);
            r(2, 0) = m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0);
            r(2, 1) = m(0, 1) * m(2, 0) - m(0, 0) * m(2, 1);
            r(2, 2) = m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
            r *= d;
         }

         return r;
      }


      //! \brief In-place rotate a matrix left 90-degrees.
      //! \param[in]    m              matrix to be rotated
      static void rotateL(Matrix<M, M, T>& m)
      {
         for (unsigned int i = 0; i < M >> 1; ++i) {
            for (unsigned int j = 0; j < (M + 1) >> 1; ++j) {
               rotationShift4(
                  m(i, j),
                  m(M - 1 - j, i),
                  m(M - 1 - i, M - 1 - j),
                  m(j, M - 1 - i));
            }
         }
      }


      //! \brief In-place rotate a matrix right 90-degrees.
      //! \param[in]    m              matrix to be rotated
      static void rotateR(Matrix<M, M, T>& m)
      {
         for (unsigned int i = 0; i < M >> 1; ++i) {
            for (unsigned int j = 0; j < (M + 1) >> 1; ++j) {
               rotationShift4(
                  m(j, M - 1 - i),
                  m(M - 1 - i, M - 1 - j),
                  m(M - 1 - j, i),
                  m(i, j));
            }
         }
      }


      //! \brief Create a vector from a row.
      //! \param[in]    row            the row to copy
      //! \return       a vector containing values from the given row; returns
      //!               zero vector if row is out of range
      Vector<N, T> rowV(unsigned int row) const
      {
         Vector<N, T> r;

         if (row < M) {
            for (unsigned int i = 0; i < N; ++i) {
               r(i) = _m[row][i];
            }
         }

         return r;
      }


      //! \brief Create a vector from a column.
      //! \param[in]    col            the column to copy
      //! \return       a vector containing values from the given column;
      //!               returns zero vector if column is out of range
      Vector<M, T> columnV(unsigned int col) const
      {
         Vector<M, T> r;

         if (col < N) {
            for (unsigned int i = 0; i < M; ++i) {
               r(i) = _m[i][col];
            }
         }

         return r;
      }


      //! \brief Get a minor matrix by excluding the given row and column.
      //! \param[in]    row            the row excluded
      //! \param[in]    col            the column excluded
      //! \return       a new matrix, formed from the unexcluded cells in the
      //!               original matrix; returns zero matrix if row or col are
      //!               out of bounds
#ifdef _MSC_VER
      Matrix<M - 1, N - 1, T> minorM(unsigned int row, unsigned int col) const
      {
         Matrix<M - 1, N - 1, T> r;
#else
      template < unsigned int P = M - 1, unsigned int Q = N - 1 >
      Matrix<P, Q, T> minorM(unsigned int row, unsigned int col) const
      {
         Matrix<P, Q, T> r;
#endif
         if (row >= M || col >= N) {
            return r;
         }

         for (unsigned int i = 0, k = 0; i < M; ++i) {
            if (i == row) {
               continue;
            }

            for (unsigned int j = 0, l = 0; j < N; ++j) {
               if (j == col) {
                  continue;
               }

               r(k, l) = _m[i][j];

               ++l;
            }

            ++k;
         }

         return r;
      }


      //! \brief Get the transpose of this matrix.
      //! \return       a new matrix, the transpose of this matrix
      Matrix<N, M, T> transpose() const
      {
         Matrix<N, M, T> r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r(j, i) = _m[i][j];
            }
         }

         return r;
      }


      //! \brief Matrix assignment operator. Copies values from another matrix.
      //! \param[in]    other          the matrix to copy
      //! \return       this
      Matrix& operator=(const Matrix& other)
      {
         for (unsigned int i = 0; i < M; ++i) {
            std::memcpy(_m[i], other._m[i], N * sizeof(T));
         }

         return *this;
      }


      //! \brief Array assignment operator. Copies values from an array.
      //! \param[in]    values         the array to copy
      //! \return       this
      Matrix& operator=(const T values[M][N])
      {
         for (unsigned int i = 0; i < M; ++i) {
            std::memcpy(_m[i], values[i], N * sizeof(T));
         }

         return *this;
      }


      //! \brief Test if equivalent to another matrix.
      //! \param[in]    other          the matrix to compare to
      //! \return       true iff equivalent
      //!
      //! This tests if two matrices are equivalent. Since many matrices use
      //! a floating-point storage, this will test if they are equivalent to
      //! within some epsilon for those types. For all other types this will
      //! be equivalent to `*this == other`.
      bool operator==(const Matrix& other) const
      {
         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               if (!fpRelativeEqual(_m[i][j], other._m[i][j])) {
                  return false;
               }
            }
         }

         return true;
      }


      //! \brief Test if not equivalent to another matrix.
      //! \param[in]    other          the matrix to compare to
      //! \return       true iff not equivalent
      //!
      //! See \ref operator== for floating-point comparison caveats.
      bool operator!=(const Matrix& other) const
      {
         return !operator==(other);
      }


      //! \brief Read-only cell accessor.
      //! \param[in]    row            the cell's row
      //! \param[in]    col            the cell's column
      //! \return       const reference to the cell at row, col
      const T& operator()(unsigned int row, unsigned int col) const
      {
      #ifdef DEBUG
         assert(row < M);
         assert(col < N);
      #endif

         return _m[row][col];
      }


      //! \brief Read or write cell accessor.
      //! \param[in]    row            the cell's row
      //! \param[in]    col            the cell's column
      //! \return       reference to the cell at row, col
      T& operator()(unsigned int row, unsigned int col)
      {
      #ifdef DEBUG
         assert(row < M);
         assert(col < N);
      #endif

         return _m[row][col];
      }


      //! \brief Casts matrix of different size or storage from this one.
      //! \return       a matrix with P rows and Q columns and storage type U
      //!
      //! Matrix cast operator. This will create a new matrix of the requested
      //! size and storage with the originating matrix's values. If the new
      //! matrix is smaller, values will be truncated. If larger, new values
      //! will be zero.
      template < unsigned int P, unsigned int Q, typename U >
      operator Matrix<P, Q, U>() const
      {
         Matrix<P, Q, U> r;
         unsigned int minR = std::min(M, P);
         unsigned int minC = std::min(N, Q);

         for (unsigned int i = 0; i < minR; ++i) {
            for (unsigned int j = 0; j < minC; ++j) {
               r(i, j) = _m[i][j];
            }
         }

         return r;
      }


      //! \brief Unary clone operator.
      //! \return       a new matrix with the same values as this
      Matrix operator+() const
      {
         return Matrix(*this);
      }


      //! \brief Matrix addition operator.
      //! \param[in]    other          the matrix to be added
      //! \return       a new matrix, summing this and another matrix's cells
      Matrix operator+(const Matrix& other) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] + other._m[i][j];
            }
         }

         return r;
      }


      //! \brief Matrix local addition operator.
      //! \param[in]    other          the matrix to be added
      //! \return       this matrix, summing this and another matrix's cells
      Matrix& operator+=(const Matrix& other)
      {
         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               _m[i][j] += other._m[i][j];
            }
         }

         return *this;
      }


      //! \brief Matrix negative operator.
      //! \return       a new matrix, with the opposite of all cell values
      Matrix operator-() const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = -_m[i][j];
            }
         }

         return r;
      }


      //! \brief Matrix subtraction operator.
      //! \param[in]    other          the matrix to be subtracted
      //! \return       a new matrix, subtracting this and another matrix's
      //!               cell values
      Matrix operator-(const Matrix& other) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] - other._m[i][j];
            }
         }

         return r;
      }


      //! \brief Matrix local subtraction operator.
      //! \param[in]    other          the matrix to be subtracted
      //! \return       this matrix, subtracting this and another matrix's cell
      //!               values
      Matrix& operator-=(const Matrix& other)
      {
         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               _m[i][j] -= other._m[i][j];
            }
         }

         return *this;
      }


      //! \brief Matrix multiplication operator.
      //! \param[in]    other          the matrix to be multiplied by
      //! \return       a new matrix, with values from this multiplied by
      //!               another matrix's
      //---------------------------------------------------------------------//
      //! First and second matrix have the same storage type.
      template< unsigned int P >
      Matrix<M, P, T> operator*(const Matrix<N, P, T>& other) const
      {
         Matrix<M, P, T> r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < P; ++j) {
               for (unsigned int k = 0; k < N; ++k) {
                  r(i, j) += _m[i][k] * other(k, j);
               }
            }
         }

         return r;
      }


      //! Result matrix will use first matrix's type.
      template< unsigned int P, typename U >
      Matrix<M, P, T> operator*(const Matrix<N, P, U>& other) const
      {
         Matrix<M, P, T> r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < P; ++j) {
               for (unsigned int k = 0; k < N; ++k) {
                  r(i, j) += _m[i][k] * other(k, j);
               }
            }
         }

         return r;
      }


      // res: Can't overload just by return-type. Changing the `other` 
      //      parameter to non-const "works", but it makes any multiplication
      //      just use the right store multiply and cast when necessary (except
      //      if other was const to start with, of course). Will have to think
      //      about this one. Might just have to say that people should cast
      //      one or the other to const if they want to force that side's 
      //      storage type.
#if 0
      //! Result matrix will use second matrix's storage type.
      template< unsigned int P, typename U >
      Matrix<M, P, U> operator*(Matrix<N, P, U>& other) const
      {
         Matrix<M, P, U> r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < P; ++j) {
               for (unsigned int k = 0; k < N; ++k) {
                  r(i, j) += _m[i][k] * other(k, j);
               }
            }
         }

         return r;
      }
#endif
      //---------------------------------------------------------------------//


      //! \brief Matrix local scalar multiplication operator.
      //! \param[in]    s              the scalar to multiply by
      //! \return       this matrix, cell values multiplied by the scalar
      template < typename S >
      Matrix& operator*=(S s)
      {
         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               _m[i][j] *= s;
            }
         }

         return *this;
      }


      //! \brief Scalar division operator.
      //! \param[in]    s              the scalar to divide by
      //! \return       a new matrix, cell values divided by the scalar
      template < typename S >
      Matrix operator/(S s)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }



      //! \brief Matrix local scalar division operator.
      //! \param[in]    s              the scalar to divide by
      //! \return       this matrix, cell values divided by the scalar
      template < typename S >
      Matrix& operator/=(S s)
      {
         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               _m[i][j] /= s;
            }
         }

         return *this;
      }


      // res: Unfortunately, scalar types can't be templated here because it
      //      creates an ambiguity in many cases. So here follows a lot of
      //      redundant text.

      //! \brief Scalar multiplication operator.
      //! \param[in]    s              the scalar to multiply by
      //! \return       a new matrix, with cell values from this matrix times
      //!               the scalar
      //---------------------------------------------------------------------//
      Matrix operator*(char s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(unsigned char s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(short s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(unsigned short s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(int s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(unsigned int s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(unsigned long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(long long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(unsigned long long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(float s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(double s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator*(long double s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }
      //---------------------------------------------------------------------//


      //! \brief Scalar multiplication operator.
      //! \param[in]    s              the scalar to multiply by
      //! \param[in]    m              the matrix multiplied
      //! \return       a new matrix, with cell values from this matrix times
      //!               the scalar
      //---------------------------------------------------------------------//
      friend Matrix operator*(char s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(unsigned char s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(short s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(unsigned short s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(int s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(unsigned int s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(long s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(unsigned long s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(long long s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(unsigned long long s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(float s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(double s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }


      friend Matrix operator*(long double s, const Matrix& m)
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = s * m._m[i][j];
            }
         }

         return r;
      }
      //---------------------------------------------------------------------//
      

      //! \brief Scalar division operator.
      //! \param[in]    s              the scalar to divide by
      //! \return       a new matrix, with cell values from this matrix divided
      //!               by the scalar
      //---------------------------------------------------------------------//
      Matrix operator/(char s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(unsigned char s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(short s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(unsigned short s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(int s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] * s;
            }
         }

         return r;
      }


      Matrix operator/(unsigned int s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(unsigned long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(long long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(unsigned long long s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(float s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(double s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] / s;
            }
         }

         return r;
      }


      Matrix operator/(long double s) const
      {
         Matrix r;

         for (unsigned int i = 0; i < M; ++i) {
            for (unsigned int j = 0; j < N; ++j) {
               r._m[i][j] = _m[i][j] /s;
            }
         }

         return r;
      }
      //---------------------------------------------------------------------//


   protected:
      T _m[M][N];


      //! \brief Rotate-shift four values.
      //! \param[in]    a              first value
      //! \param[in]    b              second value
      //! \param[in]    c              third value
      //! \param[in]    d              fourth value
      static void rotationShift4(T& a, T& b, T& c, T& d)
      {
         T v = a;
         a = b;
         b = c;
         c = d;
         d = v;
      }
   };


   //! \class Transform4
   //! \brief Describes a 3-dimensional transformation.
   //!
   //! This is not a particularly efficient implementation. Yet.
   template < typename T >
   class Transform4 : public Matrix<4, 4, T>
   {
   public:
      //! \brief Default constructor.
      Transform4()
         : Matrix<4, 4, T>()
      {
         this->_m[0][0] = 1;
         this->_m[1][1] = 1;
         this->_m[2][2] = 1;
         this->_m[3][3] = 1;
      }


      //! \brief Matrix copy constructor.
      //! \param[in]    matrix         matrix to copy
      Transform4(const Matrix<4, 4, T>& matrix)
         : Matrix<4, 4, T>(matrix)
      {
      }


      //! \brief Array parametric constructor.
      //! \param[in]    values         array of values to copy
      Transform4(T values[4][4])
         : Matrix<4, 4, T>(values)
      {
      }


      //! \brief Create rotated matrix using given angle and axis.
      //! \param[in]    m              matrix to rotate
      //! \param[in]    degree         angle to rotate by
      //! \param[in]    x              x-axis value
      //! \param[in]    y              y-axis value
      //! \param[in]    z              z-axis value
      //! \return       a new, rotated matrix
      Transform4 rotate(const Matrix<4, 4, T>& m, T degree, T x, T y, T z)
      {
         T rad = degree * TO_RAD, 
           c = std::cos(rad), s = std::sin(rad);
         T d = 1 - c,
           xd = x * d, yd = y * d,
           yxd = y * xd, zxd = z * xd, zyd = z * yd,
           xs = x * s, ys = y * s, zs = z * s;

         T v[4][4] = {
            { x * xd + c,    yxd - zs,      zxd + ys, 0 },
            {   yxd + zs,  y * yd + c,      zyd - xs, 0 },
            {   zxd - ys,    zyd + xs, z * z * d + c, 0 },
            {          0,           0,             0, 1 }
         };

         return Transform4(v) * m;
      }


      //! \brief Rotate transform using given angle and axis.
      //! \param[in]    degree         angle to rotate by
      //! \param[in]    x              x-axis value
      //! \param[in]    y              y-axis value
      //! \param[in]    z              z-axis value
      //! \return       this transform, rotated
      Transform4& rotate(T degree, T x, T y, T z)
      {
         return *this = rotate(*this, degree, x, y, z);
      }

      
      //! \brief Create scaled matrix using given scales.
      //! \param[in]    m              matrix to scale
      //! \param[in]    x              x-axis value
      //! \param[in]    y              y-axis value
      //! \param[in]    z              z-axis value
      //! \return       a new, scaled matrix
      Transform4 scale(const Matrix<4, 4, T>& m, T x, T y, T z)
      {
         T v[4][4] = {
            { x, 0, 0, 0 },
            { 0, y, 0, 0 },
            { 0, 0, z, 0 },
            { 0, 0, 0, 1 }
         };

         return Transform4(v) * m;
      }


      //! \brief Scale transform using given scales.
      //! \param[in]    x              x-axis value
      //! \param[in]    y              y-axis value
      //! \param[in]    z              z-axis value
      //! \return       this transform, scaled
      Transform4& scale(T x, T y, T z)
      {
         return *this = scale(*this, x, y, z);
      }


      //! \brief Create translated matrix using given offsets.
      //! \param[in]    m              matrix to translate
      //! \param[in]    x              x-axis value
      //! \param[in]    y              y-axis value
      //! \param[in]    z              z-axis value
      //! \return       a new, translated matrix
      Transform4 translate(const Matrix<4, 4, T>& m, T x, T y, T z)
      {
         T v[4][4] = {
            { 1, 0, 0, x },
            { 0, 1, 0, y },
            { 0, 0, 1, z },
            { 0, 0, 0, 1 }
         };

         return Transform4(v) * m;
      }


      //! \brief Translate transform using given offsets.
      //! \param[in]    x              x-axis value
      //! \param[in]    y              y-axis value
      //! \param[in]    z              z-axis value
      //! \return       this transform, translated
      Transform4& translate(T x, T y, T z)
      {
         return *this = translate(*this, x, y, z);
      }
   };


   // Convenience types -- squares only!
   typedef Matrix<2, 2, int> Matrix2i;
   typedef Matrix<3, 3, int> Matrix3i;
   typedef Matrix<4, 4, int> Matrix4i;

   typedef Matrix<2, 2, float> Matrix2f;
   typedef Matrix<3, 3, float> Matrix3f;
   typedef Matrix<4, 4, float> Matrix4f;

   typedef Matrix<2, 2, double> Matrix2d;
   typedef Matrix<3, 3, double> Matrix3d;
   typedef Matrix<4, 4, double> Matrix4d;

   typedef Transform4<float> Transform4f;
   typedef Transform4<double> Transform4d;
}

#endif

